#include "creature.hpp"
#include "glHelper.hpp"
#include "world.hpp"
#include "types.hpp"

World::World() { }

World::~World() { }

bool World::init(uint nCreature, uint nFood, uint xSize, uint ySize) {
    //srand(time(NULL));
    m_nCreature = nCreature;
    m_creatures = new Creature[m_nCreature];
    m_xSize = xSize;
    m_ySize = ySize;
    m_xScr = xSize/2;
    m_yScr = ySize/2;
    m_scale = 1;
    for (uint i = 0; i < m_nCreature; i++) {
        m_creatures[i].init(10,
                          (s_colour){(float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX},
                          /*
                          (s_vector){(i % 20) * 40 + 40, (i / 20) * 40 + 40},
                          (s_vector){0,0});
                          */
                          (s_vector){rand() % m_xSize, rand() % m_ySize},
                          (s_vector){rand() % 10 - 5, rand() % 10 - 5});
                          //*/
#ifdef DBG
        cout << "Making Creature " << i << " " <<
                                m_creatures[i].getColour()->r << " " <<
                                m_creatures[i].getColour()->g << " " <<
                                m_creatures[i].getColour()->b << " | " <<
                                m_creatures[i].getRadius() << " | " <<
                                m_creatures[i].getKinematic()->pos.x << " " <<
                                m_creatures[i].getKinematic()->pos.y << " | " <<
                                m_creatures[i].getKinematic()->vel.x << " " <<
                                m_creatures[i].getKinematic()->vel.y << " " <<endl;
#endif
    }

    return true;
}

void World::moveScreen(uint x, uint y, uint scale) {

}

void World::update() {
    //Move those Creatures!
    for (uint i = 0; i < m_nCreature; i++) {
        m_creatures[i].move();
    }

}

void World::paint() {
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f); // Clear the background of our window to white
    glClear(GL_COLOR_BUFFER_BIT); //Clear the colour buffer (more buffers later on)
    glLoadIdentity(); // Load the Identity Matrix to reset our drawing locations

    //drawCrap();

    for (uint i = 0; i < m_nCreature; i++) {
        m_creatures[i].paint();
    }
    applyCollision();

    //glFlush();    // Single Buffer Flush
    glutSwapBuffers(); // Swap the OpenGL Double buffers to the window
}

Creature * World::getCreatures () {
    return m_creatures;
}

void World::saveCreature (uint index, const char * filename) {

}

void World::loadCreature (uint index, const char * filename) {

}

void World::applyCollision() {
    //Collision Detection Algorithm
    for ( uint i = 0; i < m_nCreature; i++)
    {
        //Collision with other creatures
        for (uint j = i+1; j < m_nCreature; j++)
        {
            s_kinematic * kin1 = m_creatures[i].getKinematic();
            s_kinematic * kin2 = m_creatures[j].getKinematic();
            s_vector collisionCourse = (kin1->pos + kin1->vel) - (kin2->pos + kin2->vel);
            if (getMag(collisionCourse) < m_creatures[i].getRadius() + m_creatures[j].getRadius() )
            {
                //collided
                //cout<<"Collided("<<i<<","<<j<<"): "
                    //<<getMag(collisionCourse)
                    //<<" "<< m_creatures[i].getRadius() + m_creatures[j].getRadius() << endl;

                //Process only the orthogonal component

                //first get the scalefactor from
                float scaleFactor = dotProduct(m_creatures[i].getKinematic()->vel, collisionCourse) /
                                    getMag(collisionCourse) /
                                    getMag(collisionCourse);
                s_vector i_ortho = scaleFactor * collisionCourse;
                s_vector i_para = m_creatures[i].getKinematic()->vel - i_ortho;
                collisionCourse = -1.0 * collisionCourse;
                scaleFactor = dotProduct(m_creatures[j].getKinematic()->vel, collisionCourse) /
                                    getMag(collisionCourse) /
                                    getMag(collisionCourse);
                s_vector j_ortho = scaleFactor * collisionCourse;
                s_vector j_para = m_creatures[j].getKinematic()->vel - j_ortho;
                s_vector tmp = j_ortho;
                j_ortho = i_ortho;
                i_ortho = tmp;

                i_para = i_para + i_ortho;
                j_para = j_para + j_ortho;

                m_creatures[i].getKinematic()->vel = i_para;
                m_creatures[j].getKinematic()->vel = j_para;
            }
        }

        //Collision with the walls
        if(m_creatures[i].getKinematic()->pos.x - m_creatures[i].getRadius() < 0){
            m_creatures[i].getKinematic()->vel.x *= -1.0f;
            m_creatures[i].getKinematic()->pos.x = m_creatures[i].getRadius() + 0.01f;
        }else if(m_creatures[i].getKinematic()->pos.x + m_creatures[i].getRadius() > m_xSize){
            m_creatures[i].getKinematic()->vel.x *= -1.0f;
            m_creatures[i].getKinematic()->pos.x = m_xSize - m_creatures[i].getRadius() - 0.01f;
        }
        if(m_creatures[i].getKinematic()->pos.y - m_creatures[i].getRadius() < 0){
            m_creatures[i].getKinematic()->vel.y *= -1.0f;
            m_creatures[i].getKinematic()->pos.y = m_creatures[i].getRadius() + 0.01f;
        }else if(m_creatures[i].getKinematic()->pos.y + m_creatures[i].getRadius() > m_ySize){
            m_creatures[i].getKinematic()->vel.y *= -1.0f;
            m_creatures[i].getKinematic()->pos.y = m_ySize - m_creatures[i].getRadius() - 0.01f;
        }
    }
    //-----------------------------*/
}

void World::AttractCreatures(uint x, uint y){
	for ( uint i = 0; i < m_nCreature; i++){
		s_kinematic * kin = m_creatures[i].getKinematic();
		kin->vel.x -= (kin->pos.x - x)/1000;
		kin->vel.y -= (kin->pos.y - y)/1000;
	}
}

void World::SlowDownAll(){
	for ( uint i = 0; i < m_nCreature; i++){
		s_kinematic * kin = m_creatures[i].getKinematic();
		kin->vel.x /= 1.04;
		kin->vel.y /= 1.04;
	}
}
